/*
Automatic clinic tests using a robot arm.
Copyright (C) 2010  CIC-CPC, CHU Gabriel Montpied
All other trademarks, logos and copyrights are the property
of their respective owners.

This file is part of cpc-painbot.

cpc-painbot is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

cpc-painbot is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with cpc-painbot.  If not, see <http://www.gnu.org/licenses/>.
*/
#include "asservissement/regulateurdistance.h"


#include <QDebug>

#include "asservissement/asservissement.h"
#include "bras.h"
#include "capteur.h"

RegulateurDistance::RegulateurDistance(QObject *parent) :
    Regulateur(parent)
{
}

void RegulateurDistance::reguler(const Asservissement *systeme)
{
    Bras *bras = qobject_cast<Bras*>(systeme->asservi("bras"));
    Capteur *capteur = systeme->capteur("distance");

    Q_ASSERT_X(bras != 0, "RegulateurDistance::reguler", "Le système n'a pas de bras");
    Q_ASSERT_X(capteur != 0, "RegulateurDistance::reguler", "Le système n'a pas de capteur 'distance'");

    // Ecart par rapport à la consigne.
    // Si distance (fournie par le capteur)  > consigne, alors l'écart < 0 : le bras va remonter,
    // s'éloigner.
    // Autrement, il descend.
    qreal ecart = capteur->mesure().toDouble() - consigne_.toDouble();

    qDebug() << "RegulateurDistance::reguler, ecart =" << ecart << "mm";


    // On commande le bras.
    // On désire que le déplacement s'effectue en 100ms
    qreal vitesse = qBound(qreal(0.01), qAbs(ecart) * 10, qreal(95));
    qDebug() << "RegulateurDistance::reguler, vitesse =" << vitesse << "mm/s";
    bras->deplacer(0, 0, ecart, 0, vitesse);
}
